// rev 11/05 11:48

#include <iostream>
#include <stdio.h>
#include <vector>
#include "dgvector.h"
#include <ostream>

namespace mzslam {

using std::ostream;
struct pos{
  int X;
  int Y;
};

//defines a cell in the grid
struct gridCell{
gridCell():wallProbability(0.5), tagIndex(-1) {}
  float wallProbability;
  int tagIndex;

  friend ostream & operator<<(std::ostream & os, const gridCell & cell){
    os << cell.wallProbability;
    return os;
  }
};

//define data about a tag
struct tagData{
  int tagID;
  char tagOrientation; //u: Undefined n: North s: South w: West e: East 
  pos position;
};

class Map {
  dgvector< dgvector<gridCell> > map;
  std::vector<tagData> tagList;
 public:
  //initializer
  Map(){
                
  }
        
  //add cell will automatically call resize if the cell is out of the current bounds
  void addCell(int positionX, int positionY, int tagID,char tagOrientation , float wallProbability){
    resizeMap(positionX , positionY);
    gridCell newGridCell;
    newGridCell.wallProbability = wallProbability;
    if(tagID != -1){
      pos p0 ={-1, -1}; //TODO  -1,-1 is now valid point
      tagData newTag = {0.5, -1, p0};
      newTag.tagID = tagID;
      newTag.tagOrientation = tagOrientation;
      pos p = {positionX, positionY};                 
      newTag.position = p;
      tagList.push_back(newTag);
      newGridCell.tagIndex = tagList.size() - 1;      
    }
    map[positionY][positionX] = newGridCell;
  }

  float getPFromCell(int xPosition, int yPosition){
    return map[yPosition][xPosition].wallProbability;
  }
        
  int getTagIdFromCell(int xPosition, int yPosition){
    int index =  map[yPosition][xPosition].tagIndex;
    return tagList[index].tagID;
  }

  char getOrientationFromCell(int xPosition, int yPosition){
    int index =  map[yPosition][xPosition].tagIndex;
    return tagList[index].tagOrientation;
  }

  pos getTagPosition(int tagID){
    pos p = {-1,-1}; //need change since -1, -1 is now a valid position
    for(unsigned int i = 0; i < tagList.size(); i++){
      if(tagList[i].tagID == tagID){
	p = tagList[i].position;
	break;
      }
    }
    return p;
  }

  void print(){
    int mini;
    if(map.size() == 0 && map.negsize() == 0){
      mini = 0;       
    }
    else if(map.size() == 0){
      mini = map[-1].negsize();
    }
    else if(map.negsize() == 0){
      mini = map[0].negsize();
    }
    else{
      mini = std::max(map[0].size(), map[-1].size());
    }
    for(int i = map.negsize(); i < 0 ; i++){
      map[i].print(mini);
    }
    for(int i = 0; i < map.size() ; i++){
      map[i].print(mini);
    }
  }


 private:
        
  int sign(int x){
    if (x >= 0) return 1;
    else return -1;
  }


  void resizeMap(int X, int Y){
    map.resize(Y);
    if(Y >= 0){
      for(int i = 0; i < map.size();i++){
	map[i].resize(X);
      }
    }
    else{
      for(int i = 1; i <= abs(map.negsize());i++){
	map[sign(Y)*i].resize(X);
      }
    }
  }
};

} // end of namespace

#if 0
int main( int argc, const char* argv[] )
{
  Map m;
  m.addCell(0,0,-1, 'u', 1.0);
  m.addCell(0,1,-1, 'u', 0.7);
  //      m.print();
  m.addCell(2,-2,-1, 'u', 0.6);
  m.addCell(-2,2,-1, 'u', 0.3);
  //      m.print();
  m.addCell(-3,5,-1, 'u', 0.9);
  m.print();
}
#endif

